devisualization.util.algorithms.pathfinding.astar source code

1 /**
2  * A* algorithm implementation.
3  *
4  * License:
5  *     D version of code is under MIT. The original is under Apache 2.0.
6  * 
7  *     The MIT License (MIT)
8  *
9  *     Copyright (c) 2014 Devisualization (Richard Andrew Cattermole)
10  *
11  *     Permission is hereby granted, free of charge, to any person obtaining a copy
12  *     of this software and associated documentation files (the "Software"), to deal
13  *     in the Software without restriction, including without limitation the rights
14  *     to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15  *     copies of the Software, and to permit persons to whom the Software is
16  *     furnished to do so, subject to the following conditions:
17  *
18  *     The above copyright notice and this permission notice shall be included in all
19  *     copies or substantial portions of the Software.
20  *
21  *     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22  *     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23  *     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24  *     AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25  *     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26  *     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27  *     SOFTWARE.
28  *
29  * See_Also:
30  *		http://www.redblobgames.com/pathfinding/a-star/implementation.html
31  */
32 module devisualization.util.algorithms.pathfinding.astar;
33 import devisualization.util.algorithms.pathfinding.defs;
34 deprecated("Killing"):
35 /**
36  * Locates the next node in graph to get to a position
37  * Uses A* search algorithm
38  * Can use any sortable value type
39  *
40  * Params:
41  *    graph			=	The graph of nodes
42  *    start			=	Starting position to go to
43  *    goal			=	The end position 
44  *    came_from		=	The positions between start and end
45  *    cost_so_far	=	Sum of weights (cost) to get to goal from start
46  */
47 void a_star_search(T, U)(GridWithWeights graph, T start, T goal, out T[T] came_from, out U[T] cost_so_far) {
48 	PriorityQueue!(T, U) frontier;
49 	frontier.put(start, 0);
50 	
51 	cost_so_far[start] = 0;
52 	
53 	while(!frontier.empty) {
54 		T current = frontier.get();
55 		if (current == goal)
56 			break;
57 		
58 		foreach(next; graph.neighbors(current)) {
59 			U new_cost = cost_so_far[current] + graph.cost(current, next);
60 		
61 			if (next !in cost_so_far || new_cost < cost_so_far[next]) {
62 				cost_so_far[next] = new_cost;
63 				priority = new_cost + heuristic(goal, next);
64 				
65 				frontier.put(next, priority);
66 				visited[next] = current;
67 			}
68 		}
69 	}
70 }
71 
72 ///
73 unittest {
74 	GridWithWeights!(XY, int) grid = diagram4();
75 	
76 	XY[XY] came_from;
77 	int[XY] cost_so_far;
78 	a_star_search(grid, XY(1, 4), XY(7, 8), came_from, cost_so_far);
79 	XY[] path = reconstruct_path(came_from, XY(1, 4), XY(7, 8));
80 	
81 	// TODO: asserts
82 }